#include "printf.h"


module StrainSensorP {

	provides {
		interface Read<uint16_t>;
		interface Msp430UartConfigure;
	}
	uses {

	  interface GeneralIO  as Shutdown ;
	  interface UartStream as	SensorStream;
	  interface	Resource;

	  interface Timer<TMilli> as Timer;

	  interface Timer<TMilli> as CommandInterleavingTimer ;
	  interface Timer<TMilli> as CommandTimeoutTimer ;
	  interface Timer<TMilli> as ChannelTimeoutTimer ;


	  interface	Leds;

	}

}
implementation {

	msp430_uart_union_config_t msp430_uart_telos_config = {
		{ ubr: UBR_1MHZ_9600,
			umctl: UMCTL_1MHZ_9600,
			ssel: 0x02,
			pena: 0,
			pev: 0,
			spb: 0,
			clen: 1,
			listen: 0,
			mm: 0,
			ckpl: 0,
			urxse: 0,
			urxeie: 1,
			urxwie: 0,
			utxe : 1,
			urxe : 1}
	};


	enum{
		SINGLE_CHANNEL_TIMEOUT = 46080UL,
		STARTUP_TIME = 512,

		BUFFER_SIZE = 20,

		CMD_REQUEST_SAMPLE = 0x0,


		BOARD_OFF = 0x0 ,
		BOARD_READY = 0x1 ,
		BOARD_SAMPLING = 0x2 ,
	};

	uint8_t sensor_channel[] = {1,2,3,4,5,6} ; // available channels

//    uint8_t board_cmd[VWSG_BOARD_CMD_BUFFER_SIZE];

	uint8_t m_buffer[BUFFER_SIZE];
	uint8_t check_buffer[BUFFER_SIZE] ;


	uint8_t m_byte_received = 0 ;

	uint16_t m_value;


	bool m_busy = FALSE;

	task void read();
//	task void readDone();
	task void request_sample() ;

	task void parseTemperature() ;
	task void parseFrequency() ;

	static void sendCommand(uint8_t) ;

	void send_single_channel_request() ;

  bool setReset = TRUE;

  uint8_t m_status = BOARD_OFF ;

  uint8_t sensor_index = 0 ;

  uint8_t raw_frequency[4] ;
  uint8_t raw_temperature[4] ;
  uint8_t nl_counter = 0 ;

  uint8_t m_cmd ;

	task void request() {
		call Resource.request();
	}

	command error_t Read.read() {

		call Shutdown.makeOutput();

		if ( m_busy == TRUE ){
			printf("%s, EBUSY\n", __FUNCTION__);
			printfflush();
			return EBUSY;
		}

		m_busy = TRUE;
		sensor_index = 0 ;

		post read();

		return SUCCESS;
	}



	task void read(){


		// Power on the board
		call Leds.set( 7 );
		call Shutdown.set();

		sendCommand(CMD_REQUEST_SAMPLE) ;
//		call Timer.startOneShot( STARTUP_TIME );

	}

	event void Timer.fired(){
		if(m_status == BOARD_OFF){
			m_status = BOARD_READY ;
			post request_sample() ;
			return ;
		}
	}

	event void CommandInterleavingTimer.fired(){
		switch(m_cmd){
			case CMD_REQUEST_SAMPLE:
				post request_sample() ;
			break;
		}
	}

	event void CommandTimeoutTimer.fired(){

	}

	event void ChannelTimeoutTimer.fired(){

	}

	task void request_sample(){
		uint8_t i ;

		for(i = 0; i<BUFFER_SIZE; i++){
			m_buffer[i] = 0 ;
		}
		nl_counter = 0 ;
		atomic m_byte_received = 0 ;

		m_status = BOARD_SAMPLING ;
		printf("Request resource\n") ;
		printfflush() ;
		if ((!call Resource.isOwner()) && (call Resource.immediateRequest() != SUCCESS )) {
			post request();
			printf("%s, request()\n", __FUNCTION__);
			printfflush();
			return ;
		}
		send_single_channel_request() ;
	}


	void send_single_channel_request(){
		uint8_t cmd[] = {'G','1',13} ; // change the channel ===============================================================
		printf("Sending command\n") ;
		printfflush() ;
		call SensorStream.send( cmd, 3);
	}



//	task void readDone() {
//		uint16_t snd_value_;
//
//		atomic {
//			snd_value_ = m_value;
//		}
//
//		m_busy = FALSE;
//		m_status	= VWSG_BOARD_OFF;
//		m_byte_received = 0;
//
//		call Resource.release();
//		signal Read.readDone( SUCCESS, snd_value_ );
//	}

//	task void loadValue() {
//		uint8_t		i;
//
//#ifdef VWSG_USE_TIMEOUT
//		call Timeout.stop();
//#endif
////		call Leds.led2Toggle();
//
//
//
//		m_value = 0;
//
//		for( i=0; i < m_byte_received; i++ )
//		{
//			m_value = m_value * 10 + (m_buffer[i] - '0');
//		}
//
//
//		m_buffer[ m_byte_received ] = 0;
//		printf("%s, b:<%s> l:%u value:%u\n", __FUNCTION__, (char *)m_buffer, m_byte_received, m_value);
//		printfflush();
//
//
//		/*
//		** Check if the board was actrive and sent us
//		** a meaningful answer (that is different from "0000")
//		*/
//		if (m_value == 0)
//		{
//			post read();
//			return;
//		}
//
//
//		/*
//		** Power the board off
//		*/
//		call GIO.clr();
//		call Leds.set( 0 );
//		m_status = VWSG_BOARD_OFF;
//
//		post readDone();
//	}

task void check_valid(){
	uint8_t i ;
//	uint16_t value = 0 ;
//	printf("Check validity\n") ;
//	printfflush() ;
atomic{
	for(i = 0; i<m_byte_received; i++){

		if(m_buffer[i] != 48 && m_buffer[i] != 13 && m_buffer[i] != 10) return ;
		else{
			if(m_buffer[i] == 13 && i == 0) return ;
			else{
				sendCommand(CMD_REQUEST_SAMPLE) ;
			}
		}

//		if(m_buffer[i] == 10){
////			printf("NewLine\n") ;
////			 printfflush() ;
//			if(value == 0){
//				sendCommand(CMD_REQUEST_SAMPLE) ;
//			}
//			return ;
//		}
//		else{
//			if(m_buffer[i] != 13){ // there shouldn't be any carriage return, ask simonetti ==============================
//			 value = value*10 + (m_buffer[i] - '0') ;
//			}
//		}

	}

}
}

task void parseFrequency(){
	// Check if frequency is valid, if not there is no need to check temperature.
	uint8_t i ;
	uint16_t frequency_value = 0 ;
		atomic{
			for(i = 0; i<4 ; i++){
				frequency_value = frequency_value*10 + (raw_frequency[i]-'0') ;
			}
		}
		if(frequency_value == 0){
//			call Resource.release() ; ============================== I should release the resource, but don't know why, when doing that the board do no reply at next request.
			sendCommand(CMD_REQUEST_SAMPLE) ;
			return ;
		}

		printf("Frequency: %u\n",frequency_value) ;
		printfflush() ;

}

task void parseTemperature(){
	uint8_t i ;
		uint16_t temperature_value = 0 ;
		atomic{
			for(i = 0; i<4 ; i++){
				if(raw_temperature[i]!= 46) temperature_value = temperature_value*10+(raw_temperature[i]-'0') ;
			}
		}

		printf("Temperature: %u\n",temperature_value) ;
		printfflush() ;
}


	async event void SensorStream.receivedByte( uint8_t data ) {
//		printf("%s\n", __FUNCTION__);
//		printfflush();
		printf("_%u",data) ;
		printfflush() ;


		// expected values: 0000\n or xxxx\nx.xx\n where x are digits

		// additional chek to be removed when parser will be ready
		if(m_byte_received == 0 && (data == 10)) return ;
		if(data == 13) return ;

		if(data == 10){ // new line: call frequency check and switch to temperature.
			if(m_byte_received == 4) post parseFrequency() ;
			else post parseTemperature() ;
			return ;
		}
		if(m_byte_received < 4) raw_frequency[m_byte_received++] = data ;
		else{
			raw_temperature[(m_byte_received++) - 4] = data ;
			if(m_byte_received == 8) post parseTemperature() ;
		}

		// check newline
//		m_buffer[m_byte_received++] = data ;
//		if((data == 10) && (m_byte_received != 2)){ // newline, check the output (even if we have not read the whole string)
//			atomic{
//				if(nl_counter == 0){ // frequenct
//					frequency[0] = m_buffer[]
//
//					post check_valid() ;
//					nl_counter++ ;
//				}
//				return ;
//			}
//		}
//			if(data != 46)
//			if(new_line_counter == 0) memcpy(frequency,m_buffer) ;
//			else memcpy(temperature,m_buffer) ;
//
//			new_line_counter++ ;
//			return ;

//		if ( m_status == VWSG_BOARD_SAMPLING )
//		{
//			m_buffer[ m_byte_received ++ ] = data;
//
//			if ( m_byte_received == VWSG_SAMPLE_SIZE )
//			{
//				m_status = VWSG_BOARD_DATA_READY;
//				post loadValue();
//			}
//		}

//		m_buffer[ m_byte_received ] = 0;
//		printf("%s, s:%s l:%u\n", __FUNCTION__, (char *)m_buffer, m_byte_received);
//		printfflush();
	}

	static void sendCommand(uint8_t cm){
		m_cmd = cm ;
		call CommandInterleavingTimer.startOneShot(STARTUP_TIME) ;
	}

	async event void SensorStream.sendDone( uint8_t* buf, uint16_t len, error_t error )
	{
//		printf("SendDone: %u err: %u",len,error) ;
//		printfflush() ;
	}

	async event void SensorStream.receiveDone( uint8_t* buf, uint16_t len, error_t error ) {} // probably it is more useful...

	async command msp430_uart_union_config_t* Msp430UartConfigure.getConfig() {
		return & msp430_uart_telos_config;
	}

	event void Resource.granted() {
		post read();
	}

//	event void Timeout.fired() {
////		printf("%s \n", __FUNCTION__);
////		printfflush();
////
////		call GIO.clr();
////		call Leds.set( 0 );
////		atomic {
////			m_status	= VWSG_BOARD_OFF;
////			m_byte_received = 0;
////			m_busy = FALSE;
////		}
////
////		call Resource.release();
////		signal Read.readDone( FAIL, 0xFFFF );
//	}

}
